/*
 www.pipoisu.com
 Copyright (c) 2011.  All rights reserved.

 File     : mixer.cpp
 Author(s): Erez Raviv

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program. If not, see <http://www.gnu.org/licenses/>.
*/

#include <math.h>
#include "mixer.h"
#include "pipo.h"
#include "pwm.h"
#include "sensors.h"
#include "uart.h"

uint8_t thrLock = true;


void mixer()
{	
	// Set up for an X Quad
	// front right and back left CCW rotation
	// channel 2 (pitch) is reversed i.e. pull back is positive rotation
	
	/*
	 * pwmInRaw returns a value from -1024 - 0 - +1024
	 * we need to produce a similar output for each motor
	 * the input into the mixer is a desired rate (from teh controller RX or stable controller)
	 * the throttle will be at a set % and then teh control will + or - a given amount
	 * if we assume hover at ~50% throttle that gives us max of +/- 1000 ticks control
	 */
	
    //static float errIntegral[3]; // store error rotation rate integral x,y,z
    static int16_t lastErrorRate[3];
	static uint8_t loopCount=0;	// used for flashing the red/green LEDs
    int16_t Kp[3];//, Ki, Kd;
	int16_t errorRate[3];
    int16_t cmd[3],thr;
    int32_t PTerm, ITerm, DTerm;
	int16_t axisPID[3];
	
	uart_TxInt16((uint16_t)(deltaT*1000000)); // deltaT in us
	
	/*
	 * Kp, Ki & Kd - the gains for the inner PID controller
	 * this controller takes a commanded rotation rate about all 3 axis
	 * compares it with the measured rate and then outputs the motor values
	 * The mixer adds to 1/2 the motors and subtracts from the other 1/2
	 * This behavior may need changing if the quad loses or gains height during maneuvers
	 *
	 * This routine tries to use as many ints as possible rather than lots of floating point arithmetic
	 * hence why we are dealing with milli radians etc.
	 * this does mean that the Kp, Ki & Kd gains will not be similar in magnitude to any other controller
	 */
	
    Kp[0] = Kp[1] = (1024+pwmInRaw(5)/2);	// results in 512...1536 int
													// trials have shown 1000 to work well
	Kp[2] = 4096 + (pwmInRaw(6)*2);		// results in 2048..6134 int
													// trials have shown 7000 to work well
													// actually 7000 is too aggressive
	
    //Ki = (pwmInRaw(6) + RESX)/20; //###### should we constrain this here!!! - should this be 0...100?
	//Ki = (pwmInRaw(6))/20; //###### should we constrain this here!!! - should this be 0...100?	
    //Kd = (pwmInRaw(7) + RESX)/2;  // constrained by design (the dTerm is by definition small)
	//Kd = (pwmInRaw(7))/2;  // constrained by design (the dTerm is by definition small)
    
	uart_TxInt16(Kp[0]); uart_TxInt16(Kp[1]); uart_TxInt16(Kp[2]); // 6 bytes // ***** LOGGER *****
	
	/* ####TODO####
	 * incorporate PD into stick commands
	 * Using PD on stick movements will make the reaction much more aggressive for faster stick movements 
	 * and more gentle for slow movements
	 *
	 * we now have pwm_in_delta array which holds the delta to the previous stick measurement
	 * so gradient = pwm_in_delta/pwm_deltaT 
	 * pwm_deltaT needs to be measured - it is the RX frame rate. On FlySky it is fixed
	 * We will also need a stickInKp & stickInKd vars (eeprom)
	 */
	
    cmd[cR] = ((pwmInRaw(1) - pwmInBias(1)) * maxCmdRadPerSec); // this will result in an int in milli rad/s 
    cmd[cP] = ((pwmInRaw(2) - pwmInBias(2)) * maxCmdRadPerSec); // this will result in an int in milli rad/s
    cmd[cY] = ((pwmInRaw(4) - pwmInBias(4)) * maxCmdRadPerSec); // this will result in an int in milli rad/s
    thr		= pwmInRaw(3); // -RESX...+RESX (-1024...+1024)
	
	uart_TxInt16(thr); // ***** LOGGER *****
    
	for(uint8_t axis=cR;axis<=cY;axis++) {
	    
		errorRate[axis] = cmd[axis] - int16_t(sensor_data[axis+SEN_GX]); // error rate in milli rad/s
		// realistically this will result in a number say +/- 12,000 = ~2 rev/s
		// more likely +/- 6,000 i.e. 1 rev/sec
		uart_TxInt16(cmd[axis]); uart_TxInt16(int16_t(sensor_data[axis+SEN_GX])); // ***** LOGGER *****
		
        //errIntegral[axis]  += (float(errorRate[axis]))*deltaT; // 6,000 * 0.005 = 30 ***per loop*** > 6000 per sec
        // constraining to 20,000 ##### this needs to be properly thought out!!
        //errIntegral[axis]   = CONSTRAIN(errIntegral[axis],20000); //prevent WindUp 
		
		PTerm    = Kp[axis-cR] * int32_t(errorRate[axis]); // value up to +/-6,000,000 - this should be dominant value
		ITerm    = 0;//Ki * int32_t(((thr+RESX)/10)	* (int16_t)errIntegral[axis]); // should we also factor to thr% - probably!!
		DTerm    = 0;//Kd * int32_t((errorRate[axis] - lastErrorRate[axis]) / deltaT);	// deltaT will be ~0.005s
																					// for an acceleration of 0->6 rad/s in 0.2s
																					// Dterm = Kd * 30000
		lastErrorRate[axis] = errorRate[axis]; // in mrad/s

        axisPID[axis] = int16_t((PTerm + ITerm + DTerm)/10000) ;//translates to integer, max hopefully around teh +/- 500 -> 1,000 range
		
		uart_TxInt16(axisPID[axis]); // ***** LOGGER *****
    }

	int16_t frontRight = CONSTRAIN(thr - axisPID[cR] + axisPID[cP] + axisPID[cY],RESX);
    int16_t backRight  = CONSTRAIN(thr - axisPID[cR] - axisPID[cP] - axisPID[cY],RESX);
    int16_t backLeft   = CONSTRAIN(thr + axisPID[cR] - axisPID[cP] + axisPID[cY],RESX);
    int16_t frontLeft  = CONSTRAIN(thr + axisPID[cR] + axisPID[cP] - axisPID[cY],RESX);

	
/*	// Log actual motor commands
	uart_TxInt16(frontRight);uart_TxInt16(backRight);
	uart_TxInt16(backLeft);uart_TxInt16(frontLeft);
*/	
	uart_TxInt16(0xAAAA); // LOGGER end packet delimiter

   if(thr<(MOTOR_IDLE)) { // needed for safety at the moment!!
        frontRight = -RESX;
        backRight  = -RESX;
        backLeft   = -RESX;
        frontLeft  = -RESX;  
    }

    if(thrLock) {
		// test for arm if sticks are bottom right - if so then arm
		// remember, channel 2 (pitch) is reversed
        thrLock = (pwmInRaw(4)<(STICK_HI)) || (thr>(STICK_LO)) || 
				  (pwmInRaw(2)<(STICK_HI)) || (pwmInRaw(1)<(STICK_HI)); 
    
	    pwm_out_set(MOTOR_FR,-RESX);		pwm_out_set(MOTOR_BR,-RESX);
		pwm_out_set(MOTOR_BL,-RESX);		pwm_out_set(MOTOR_FL,-RESX);
		
		LED_RED_OFF;if(--loopCount==0) LED_GREEN_FLIP;
		
		if ((pwmInRaw(4)<(STICK_LO)) && (thr<(STICK_LO)) && 
			(pwmInRaw(1)<(STICK_LO)) && (pwmInRaw(2)<(STICK_LO))) {	// test for soft reset
				wdt_enable(WDTO_15MS);
				for(;;) {} // endless loop waiting for the reset
		}
	} else {
		// test for disarm if left sticks bottom left - if so disarm
		// channel 2 (pitch) is reversed
		// otherwise set motors to mixer amount or at least idle them
		thrLock = (pwmInRaw(4)<(STICK_LO)) && (thr<(STICK_LO)); // &&
				  //(pwmInRaw(2)>(STICK_HI)) && (pwmInRaw(1)<(STICK_LO)); 
				  
		pwm_out_set(MOTOR_FR,MAX(frontRight, MOTOR_IDLE));
		pwm_out_set(MOTOR_BR,MAX(backRight,  MOTOR_IDLE));
		pwm_out_set(MOTOR_BL,MAX(backLeft,   MOTOR_IDLE));
		pwm_out_set(MOTOR_FL,MAX(frontLeft,  MOTOR_IDLE));
		
		LED_GREEN_OFF; if(--loopCount==0) LED_RED_FLIP;
	}
}
